'''
Author: SlytherinGe
LastEditTime: 2021-04-29 19:59:54
'''
import copy
import warnings

import torch
import torch.nn as nn
import torch.nn.functional as F
from mmcv import ConfigDict
from mmcv.cnn import normal_init
from mmcv.ops import batched_nms

if __name__ == '__main__':
    from mmdet.models.builder import HEADS
    from mmdet.models.dense_heads.anchor_head import AnchorHead
else:
    from ..builder import HEADS
    from .anchor_head import AnchorHead

@HEADS.register_module()
class APNHead(AnchorHead):
    def __init__(self, in_channels, **kwargs):
        super(APNHead, self).__init__(1, in_channels, **kwargs)

    def _init_layers(self):

        self.apn_conv = nn.Conv2d(self.in_channels, self.feat_channels, 3, padding=1)
        self.apn_cls = nn.Conv2d(self.feat_channels,
                                 self.num_anchors*self.cls_out_channels, 1)
        # only change position, instead of change size
        self.apn_reg = nn.Conv2d(self.feat_channels, self.num_anchors * 2, 1)


    def init_weights(self):
        """Initialize weights of the head."""
        normal_init(self.apn_conv, std=0.01)
        normal_init(self.apn_cls, std=0.01)
        normal_init(self.apn_reg, std=0.01)    


    def forward_single(self, x):
        # TODO: add some non-linear property
        """Forward feature map of a single scale level."""
        x = self.apn_conv(x)
        x = F.relu(x, inplace=True)
        apn_cls_score = self.apn_cls(x)
        apn_bbox_pred = self.apn_reg(x)
        return apn_cls_score, apn_bbox_pred

    def _get_bboxes(self,
                    cls_scores,
                    bbox_preds,
                    mlvl_anchors,
                    img_shapes,
                    scale_factors,
                    cfg,
                    rescale=False):
        """Transform outputs for a single batch item into bbox predictions.

        Args:
            cls_scores (list[Tensor]): Box scores for each scale level
                Has shape (N, num_anchors * num_classes, H, W).
            bbox_preds (list[Tensor]): Box deltas for each scale
                level with shape (N, num_anchors * 2, H, W).
            mlvl_anchors (list[Tensor]): Box reference for each scale level
                with shape (num_total_anchors, 4).
            img_shapes (list[tuple[int]]): Shape of the input image,
                (height, width, 3).
            scale_factors (list[ndarray]): Scale factor of the image arange as
                (w_scale, h_scale, w_scale, h_scale).
            cfg (mmcv.Config): Test / postprocessing configuration,
                if None, test_cfg would be used.
            rescale (bool): If True, return boxes in original image space.

        Returns:
            list[tuple[Tensor, Tensor]]: Each item in result_list is 2-tuple.
                The first item is an (n, 5) tensor, where the first 4 columns
                are bounding box positions (tl_x, tl_y, br_x, br_y) and the
                5-th column is a score between 0 and 1. The second item is a
                (n,) tensor where each item is the predicted class label of the
                corresponding box.
        """
        cfg = self.test_cfg if cfg is None else cfg
        cfg = copy.deepcopy(cfg)
        # bboxes from different level should be independent during NMS,
        # level_ids are used as labels for batched NMS to separate them
        level_ids = []
        mlvl_scores = []
        mlvl_bbox_preds = []
        mlvl_valid_anchors = []
        batch_size = cls_scores[0].shape[0]
        nms_pre_tensor = torch.tensor(
            cfg.nms_pre, device=cls_scores[0].device, dtype=torch.long)
        for idx in range(len(cls_scores)):
            # for each scale level
            rpn_cls_score = cls_scores[idx]
            rpn_bbox_pred = bbox_preds[idx]
            # make sure they are of the same height and width
            assert rpn_cls_score.size()[-2:] == rpn_bbox_pred.size()[-2:]
            # transfer the shape into (N, H, W, num_anchors * 2)
            rpn_cls_score = rpn_cls_score.permute(0, 2, 3, 1)
            # transfer the shape into (N, H * W, num_anchors * 2)
            if self.use_sigmoid_cls:
                rpn_cls_score = rpn_cls_score.reshape(batch_size, -1)
                scores = rpn_cls_score.sigmoid()
            else:
                rpn_cls_score = rpn_cls_score.reshape(batch_size, -1, 2)
                # We set FG labels to [0, num_class-1] and BG label to
                # num_class in RPN head since mmdet v2.5, which is unified to
                # be consistent with other head since mmdet v2.0. In mmdet v2.0
                # to v2.4 we keep BG label as 0 and FG label as 1 in rpn head.
                scores = rpn_cls_score.softmax(-1)[..., 0]
            rpn_bbox_pred = rpn_bbox_pred.permute(0, 2, 3, 1).reshape(
                batch_size, -1, 2)
            anchors = mlvl_anchors[idx]
            anchors = anchors.expand(batch_size, -1, 2)
            if nms_pre_tensor > 0:
                # sort is faster than topk
                # _, topk_inds = scores.topk(cfg.nms_pre)
                # keep topk op for dynamic k in onnx model
                if torch.onnx.is_in_onnx_export():
                    # sort op will be converted to TopK in onnx
                    # and k<=3480 in TensorRT
                    scores_shape = torch._shape_as_tensor(scores)
                    nms_pre = torch.where(scores_shape[1] < nms_pre_tensor,
                                            scores_shape[1], nms_pre_tensor)
                    _, topk_inds = scores.topk(nms_pre)
                    batch_inds = torch.arange(batch_size).view(
                        -1, 1).expand_as(topk_inds)
                    scores = scores[batch_inds, topk_inds]
                    rpn_bbox_pred = rpn_bbox_pred[batch_inds, topk_inds, :]
                    anchors = anchors[batch_inds, topk_inds, :]

                elif scores.shape[-1] > cfg.nms_pre:
                    ranked_scores, rank_inds = scores.sort(descending=True)
                    topk_inds = rank_inds[:, :cfg.nms_pre]
                    scores = ranked_scores[:, :cfg.nms_pre]
                    batch_inds = torch.arange(batch_size).view(
                        -1, 1).expand_as(topk_inds)
                    rpn_bbox_pred = rpn_bbox_pred[batch_inds, topk_inds, :]
                    anchors = anchors[batch_inds, topk_inds, :]

            mlvl_scores.append(scores)
            mlvl_bbox_preds.append(rpn_bbox_pred)
            mlvl_valid_anchors.append(anchors)
            level_ids.append(
                scores.new_full((
                    batch_size,
                    scores.size(1),
                ),
                                idx,
                                dtype=torch.long))

        batch_mlvl_scores = torch.cat(mlvl_scores, dim=1)
        batch_mlvl_anchors = torch.cat(mlvl_valid_anchors, dim=1)
        # add wh shift which is 0
        batch_mlvl_rpn_bbox_pred = torch.cat(mlvl_bbox_preds, dim=1)
        batch_mlvl_rpn_bbox_delta_size = torch.zeros_like(batch_mlvl_rpn_bbox_pred)
        batch_mlvl_rpn_bbox_pred = torch.cat((batch_mlvl_rpn_bbox_pred
                                            ,batch_mlvl_rpn_bbox_delta_size),
                                            dim=-1)

        batch_mlvl_proposals = self.bbox_coder.decode(
            batch_mlvl_anchors, batch_mlvl_rpn_bbox_pred, max_shape=img_shapes)
        batch_mlvl_ids = torch.cat(level_ids, dim=1)

        # deprecate arguments warning
        if 'nms' not in cfg or 'max_num' in cfg or 'nms_thr' in cfg:
            warnings.warn(
                'In rpn_proposal or test_cfg, '
                'nms_thr has been moved to a dict named nms as '
                'iou_threshold, max_num has been renamed as max_per_img, '
                'name of original arguments and the way to specify '
                'iou_threshold of NMS will be deprecated.')
        if 'nms' not in cfg:
            cfg.nms = ConfigDict(dict(type='nms', iou_threshold=cfg.nms_thr))
        if 'max_num' in cfg:
            if 'max_per_img' in cfg:
                assert cfg.max_num == cfg.max_per_img, f'You ' \
                    f'set max_num and ' \
                    f'max_per_img at the same time, but get {cfg.max_num} ' \
                    f'and {cfg.max_per_img} respectively' \
                    'Please delete max_num which will be deprecated.'
            else:
                cfg.max_per_img = cfg.max_num
        if 'nms_thr' in cfg:
            assert cfg.nms.iou_threshold == cfg.nms_thr, f'You set' \
                f' iou_threshold in nms and ' \
                f'nms_thr at the same time, but get' \
                f' {cfg.nms.iou_threshold} and {cfg.nms_thr}' \
                f' respectively. Please delete the nms_thr ' \
                f'which will be deprecated.'

        result_list = []
        for (mlvl_proposals, mlvl_scores,
                mlvl_ids) in zip(batch_mlvl_proposals, batch_mlvl_scores,
                                batch_mlvl_ids):
            # Skip nonzero op while exporting to ONNX
            if cfg.min_bbox_size > 0 and (not torch.onnx.is_in_onnx_export()):
                w = mlvl_proposals[:, 2] - mlvl_proposals[:, 0]
                h = mlvl_proposals[:, 3] - mlvl_proposals[:, 1]
                valid_ind = torch.nonzero(
                    (w >= cfg.min_bbox_size)
                    & (h >= cfg.min_bbox_size),
                    as_tuple=False).squeeze()
                if valid_ind.sum().item() != len(mlvl_proposals):
                    mlvl_proposals = mlvl_proposals[valid_ind, :]
                    mlvl_scores = mlvl_scores[valid_ind]
                    mlvl_ids = mlvl_ids[valid_ind]

            dets, keep = batched_nms(mlvl_proposals, mlvl_scores, mlvl_ids,
                                        cfg.nms)
            result_list.append(dets[:cfg.max_per_img])
        return result_list

    def loss_single(self, cls_score, bbox_pred, anchors, labels, label_weights,
                    bbox_targets, bbox_weights, num_total_samples):
        """Compute loss of a single scale level.

        Args:
            cls_score (Tensor): Box scores for each scale level
                Has shape (N, num_anchors * num_classes, H, W).
            bbox_pred (Tensor): Box energies / deltas for each scale
                level with shape (N, num_anchors * 2, H, W).
            anchors (Tensor): Box reference for each scale level with shape
                (N, num_total_anchors, 4).
            labels (Tensor): Labels of each anchors with shape
                (N, num_total_anchors).
            label_weights (Tensor): Label weights of each anchor with shape
                (N, num_total_anchors)
            bbox_targets (Tensor): BBox regression targets of each anchor wight
                shape (N, num_total_anchors, 4).
            bbox_weights (Tensor): BBox regression loss weights of each anchor
                with shape (N, num_total_anchors, 4).
            num_total_samples (int): If sampling, num total samples equal to
                the number of total anchors; Otherwise, it is the number of
                positive anchors.

        Returns:
            dict[str, Tensor]: A dictionary of loss components.
        """
        # classification loss
        labels = labels.reshape(-1)
        label_weights = label_weights.reshape(-1)
        cls_score = cls_score.permute(0, 2, 3,
                                      1).reshape(-1, self.cls_out_channels)
        loss_cls = self.loss_cls(
            cls_score, labels, label_weights, avg_factor=num_total_samples)
        # regression loss
        bbox_targets = bbox_targets.reshape(-1, 4)
        bbox_weights = bbox_weights.reshape(-1, 4)
        bbox_pred = bbox_pred.permute(0, 2, 3, 1).reshape(-1, 4)
        bbox_delta_size = torch.zeros_like(bbox_pred)
        bbox_pred = torch.cat((bbox_pred, bbox_delta_size), dim=-1)
        if self.reg_decoded_bbox:
            # When the regression loss (e.g. `IouLoss`, `GIouLoss`)
            # is applied directly on the decoded bounding boxes, it
            # decodes the already encoded coordinates to absolute format.
            anchors = anchors.reshape(-1, 4)
            bbox_pred = self.bbox_coder.decode(anchors, bbox_pred)
        loss_bbox = self.loss_bbox(
            bbox_pred,
            bbox_targets,
            bbox_weights,
            avg_factor=num_total_samples)
        return loss_cls, loss_bbox

    def __aoi_pygenerate(self, img_shape, gt_bboxes, aoi_size):

        gt_aois = []
        for gt_bbox in gt_bboxes:
            aois = []
            gt_aoi = []
            im_w, im_h, _ = img_shape
            # sort valid gt boxes
            w_valid = gt_bbox[:,2] - gt_bbox[:,0] <= aoi_size
            h_valid = gt_bbox[:,3] - gt_bbox[:,1] <= aoi_size
            valid_index = w_valid & h_valid
            valid_gt = gt_bbox[valid_index]
            oversized_gt = gt_bbox[~valid_index]
            # allocate all bboxes into aois
            while(valid_gt.shape[0] != 0):
                remove_index = []
                temp = lefttop_rightdown_2_center_width(valid_gt[0])
                aoi = torch.tensor((temp[0] - aoi_size/2, temp[1] - aoi_size/2, temp[0] + aoi_size/2, temp[1] + aoi_size/2))
                remove_index.append(0)
                aoi_dict = {
                        'aoi': aoi,
                        'bbox' : [valid_gt[0]]
                }
                for i in range(1, valid_gt.shape[0]):
                    if is_small_box_in_big(valid_gt[i], aoi):
                        aoi_dict['bbox'].append(valid_gt[i])
                        remove_index.append(i)
                # remove used bbox
                mask = [True for _ in range(valid_gt.shape[0])]
                for i in remove_index:
                    mask[i] = False
                valid_gt = valid_gt[mask]
                aois.append(aoi_dict)
            # refine aois. make the group center of bboxes in the center of aoi
            # and then make sure all the aois are in the range of the image
            for k, aoi in enumerate(aois):
                bbox_center = []
                cx, cy = 0, 0
                for bbox in aoi['bbox']:
                    bbox_center.append(lefttop_rightdown_2_center_width(bbox)[:2])
                for center in bbox_center:
                    cx += center[0]
                    cy += center[1]
                cx /= len(bbox_center)
                cy /= len(bbox_center)
                aois[k]['aoi'] = (cx - aoi_size/2, cy - aoi_size/2, cx + aoi_size/2, cy + aoi_size/2)
                aois[k]['aoi'] = clip_bbox(aois[k]['aoi'], (im_w, im_h))
                gt_aoi.append(aois[k]['aoi'])
            gt_aois.append(torch.stack(gt_aoi, dim=0).cuda())

        return gt_aois

    def forward_train(self,
                      x,
                      img_metas,
                      gt_bboxes,
                      gt_labels=None,
                      gt_bboxes_ignore=None,
                      proposal_cfg=None,
                      **kwargs):
        """
        Args:
            x (list[Tensor]): Features from FPN.
            img_metas (list[dict]): Meta information of each image, e.g.,
                image size, scaling factor, etc.
            gt_bboxes (Tensor): Ground truth bboxes of the image,
                shape (num_gts, 4).
            gt_labels (Tensor): Ground truth labels of each box,
                shape (num_gts,).
            gt_bboxes_ignore (Tensor): Ground truth bboxes to be
                ignored, shape (num_ignored_gts, 4).
            proposal_cfg (mmcv.Config): Test / postprocessing configuration,
                if None, test_cfg would be used

        Returns:
            tuple:
                losses: (dict[str, Tensor]): A dictionary of loss components.
                proposal_list (list[Tensor]): Proposals of each image.
        """
        outs = self(x)

        gt_bboxes = self.__aoi_pygenerate(img_metas[0]['img_shape'], gt_bboxes, 100)
        
        loss_inputs = outs + (gt_bboxes, gt_labels, img_metas)
        losses = self.loss(*loss_inputs, gt_bboxes_ignore=None)
        if proposal_cfg is None:
            return losses
        else:
            proposal_list = self.get_bboxes(*outs, img_metas, cfg=proposal_cfg)
            return losses, proposal_list
    


def is_small_box_in_big(small_box, big_box):
    '''
    smal_box:tensor(xmin, ymin, xmax, ymax)
    big_box:tensor(xmin, ymin, xmax, ymax)
    '''
    min_x = small_box[0] >= big_box[0]
    min_y = small_box[1] >= big_box[1]
    max_x = small_box[2] <= big_box[2]
    max_y = small_box[3] <= big_box[3]
    return bool(min_x & min_y & max_x & max_y)

def lefttop_rightdown_2_center_width(bbox):
    '''
    bbox: tensor(xmin, ymin, xmax, ymax)
    return: tensor(xmid, ymid, width, height)
    '''
    return torch.tensor(((bbox[0]+bbox[2])/2, (bbox[1]+bbox[3])/2, bbox[2]-bbox[0], bbox[3]-bbox[1]))

def clip_bbox(bbox, im_shape):
    '''
    make sure all the input bbox is in the range of the im_shape and return int number
    param: bbox: tuple(xmin, ymin, xmax, ymax)
    param: im_shape: tuple(im_width, im_height)
    return: the bbox in the im_shape
    '''
    xmin, ymin, xmax, ymax = bbox
    if bbox[0] < 0:
        xmin = 0
    if bbox[1] < 0:
        ymin = 0
    if bbox[2] >= im_shape[0]:
        xmax = im_shape[0] - 1
    if bbox[3] >= im_shape[1]:
        ymax = im_shape[1] - 1
    return torch.tensor((int(xmin), int(ymin), int(xmax), int(ymax)))


